#include "chassis_task.h"
#include "FreeRTOS.h"
#include "task.h"
#include "cmsis_os.h"
#include "chassis.h"
#include "can_motor_user.h"
#include "locate_task.h"
#include "uart.h"
#include "can_motor_user.h"
#include "pid.h"
float x;
float y;
float z;
float p;


void ChassisTask (void *argument);
osThreadId_t	chassisTaskHandle ;		//电机调速任务（速度环pid调速）
osThreadAttr_t chassisTask_attributes;
//底盘任务创建
extern "C" void chassis_task_init()
{
    //配置角速度修正pid类的参数
    {
        const float MAX_z = 20.0f;		//最大输出的z
        float p = 0.1, i = 0.1f, d = 0.0, f = 0;
        float intergral_lim = 200, deadzone = 0.001f;
        chassis::angular_pid.pid_param_init(pid_t::PID_ENUM::POSITION_PID,
                                     pid_t::PID_ENUM::D_FIRST_EN,
                                     pid_t::PID_ENUM::DEADBAND_CLEAR_I,
                                     MAX_z,	intergral_lim,
                                     deadzone,
                                     p ,i, d, f);
    }
    //创建任务
    {
        chassisTask_attributes.name = "chassisTask";
        chassisTask_attributes.stack_size = 128 * 4;
        chassisTask_attributes.priority = (osPriority_t) osPriorityNormal;		//优先级normal
    }
    chassisTaskHandle = osThreadNew (ChassisTask , NULL, &chassisTask_attributes  );
}

int start = 0;//暂停键
extern uint8_t error;
extern uint8_t bluetooth_flag;
extern uint8_t xbox_A;
//extern uint8_t xbox_Up;       //目前与发射冲突了
//extern uint8_t xbox_Left;     //目前与发射冲突了
extern float Z;

/**
 * @brief 底盘输出任务
 */
void ChassisTask ( void *argument)
{
    osDelay(500);   //初始化后先延时500ms
    for(;;)
    {

        if(error)   //错误时,全部电机停止输出
        {
            chassis::all_stop();
        }
        else
        {
            if(bluetooth_flag)//蓝牙状态正常
            {

                if(xbox_A==0)
                {
                    chassis::control(x,y,z);
                }

                if(xbox_A==1)
                {

                    chassis::all_stop();	//停止输出
                }

#if 0
                if (xbox_Up==1)  //自瞄
                {
                    chassis_cal_and_output(0,0,z);
                }
#endif
            }
                //蓝牙断联时,底盘输出速度全部为0
            else
            {
                chassis::control(0,0,0);
            }
        }

        //RTOS释放1ms CPU
        osDelay(1);
    }
}


